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In  the  absence  of  a  global  frame  of  reference,  the  ability  to  fuse  data  collected 
by  multiple  mobile  agents  that  operate  in  separate  coordinate  systems  is  criti¬ 
cal  for  enabling  autonomy  in  multi-agent  navigation  and  perception  systems.  Of 
particular  interest  is  the  ability  to  fuse  rigid  body  metric  environment  models  in 
order  to  construct  a  global  model  from  the  data  collected  by  each  agent.  This 
thesis  presents  a  data  fusion  approach  for  combining  Gaussian  metric  models  of 
an  environment  constructed  by  multiple  agents  that  operate  outside  of  a  global 
reference  frame.  Common  landmarks  are  combined  using  a  nonlinear  least  squares 
approximation,  which  yields  an  exact  solution  under  the  assumption  of  isotropic 
covariance.  Rigid  body  transform  parameters  and  common  landmarks  are  found 
using  a  hypergraph  registration  approach.  The  approach  demonstrates  a  robust¬ 
ness  to  outliers  in  registration  by  incorporating  unit  quaternions  to  reject  outliers 
on  a  unit  sphere.  The  performance  of  the  approach  is  evaluated  using  experi¬ 
mental  benchmark  datasets  collected  in  natural  and  semi-structured  environments 
with  camera  and  laser  sensors. 
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LIST  OF  FIGURES 

1.1  Illustration  of  mobile  robotic  agents.  Land  rovers  such  as  (left) 
Pioneer  robots,  (center)  Segways  and  (right)  the  Mars  Curiosity 
rover  are  examples  of  mobile  agents  that  operate  in  environments 
with  limited  access  to  global  reference  frames.  The  main  topic  of 
this  thesis  is  the  data  fusion  problem  of  constructing  a  global  metric 
map  from  rigid-body  data  models  obtained  in  the  reference  frames 

of  each  agent .  2 

1.2  A  data  fusion  problem  involving  rigid-body  data  models.  The  data 
models  of  agent  p  (bottom  left)  and  agent  q  (bottom  right)  illus¬ 
trate  landmark  locations  estimated  from  the  sensor  observations  of 
two  mobile  agents  operating  ontside  of  a  global  reference  frame. 
Uncertainties  in  estimation  are  indicated  by  ellipses  and  the  path 
of  exploration  is  shown  by  dotted  lines.  Estimation  of  a  common 
global  map  from  the  individual  data  models,  each  obtained  in  a 
separate  coordinate  system,  requires  inferring  common  landmarks 

in  addition  to  determining  a  common  frame  of  reference .  3 

2.1  Illustration  of  anisotropic  data  fusion:  (top)  x  and  (bottom)  y  loca¬ 
tions.  Three  landmarks  (vertical  black  lines)  located  at  the  coordi¬ 
nates  (-15,-15),  (0,0)  and  (15,15)  are  estimated  individually  by 
a  fusion  agent  (bold  lines)  and  a  contributing  agent  (dashed  lines). 

Data  fusion  enables  the  fusion  agent  to  estimate  the  location  of 
landmarks  by  incorporating  the  data  of  a  contributing  agent  (illus¬ 
trated  in  red).  The  peak  of  each  distribution  corresponds  to  the 
estimated  landmark  location,  while  the  width  indicates  the  uncer¬ 
tainty  of  the  estimate .  22 

3.1  Cramer- Rao  lower  bounds  and  performance  of  closed- form  MLE. 

(top)  A  plot  of  the  trace  of  the  CRLB  covariance  matrices  Plt)ll , 

Pt  t  and  Pep  derived  from  the  parameters  p  (common  landmarks), 
t  (translation)  and  6  (rotation),  respectively,  is  shown  above  in  the 
figure.  The  plots  are  shown  against  the  variance  of  the  closed-form 

ML  estimators  p*,  t*  and  6* .  34 

4.1  Overview  of  data  registration  approach.  Common  landmarks  and 
rigid-body  transform  parameters  are  determined  using  a  directed 
hypergraph  representation  of  metric  maps.  Likelihood  statistics  of 
common  directed  triangles  are  stored  in  a  matrix  A,  which  is  used 
to  determine  common  landmarks  by  solving  a  linear  assignment 
problem.  The  solution  of  the  assignment  problem  is  partitioned 

into  inkers  and  outliers  using  a  quaternion  ontlier  rejection  approach.  36 
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4.2  Monte  Carlo  performance  of  hypergraph  hypothesis  testing.  Re¬ 
ceiver  operating  characteristic  (ROC)  curves,  illustrated  above, 
show  the  performance  of  the  detecting  triangle  matches  at  vari¬ 
ous  levels  of  SNR.  Each  of  the  curves  are  plots  of  the  probability 
of  detecting  a  match  (Pd)  versus  the  probability  of  a  false  alarm 
(Pfa).  The  dashed  line  in  the  lower  region  of  the  figure  indicates 

the  performance  of  a  random  guess .  38 

4.3  Benchmark  rigid-body  models  of  natural  and  semi-structured  en¬ 

vironments.  (a)  Victoria  Park  provides  an  example  of  a  natural 
outdoor  environment  (dataset  courtesy  of  the  University  of  Syd¬ 
ney).  Landmark  locations  in  the  environment  are  determined  us¬ 
ing  a  laser  range  finder,  (b)  An  example  of  a  semi-structured  in¬ 
door  environment  is  provided  by  the  DLR  Institute  of  Robotics  and 
Mechatronics  building  (dataset  courtesy  of  the  LIniversity  of  Bre¬ 
men).  In  contrast  to  the  Victoria  Park  dataset,  a  camera  sensor 
is  used  to  determine  the  locations  of  landmarks  within  a  research 
facility.  .  40 

4.4  Synthetic  models  of  natural  and  semi-structured  environments,  (a) 

Samples  drawn  from  a  uniform  distribution  provide  a  model  of  land¬ 
marks  within  a  natural  environment,  (b)  Landmarks  in  a  semi- 
structured  environment  are  modeled  in  this  example  by  points  in 
a  deterministic  grid  with  additive  noise.  Each  of  the  examples 
provide  simple  models  of  the  spatial  arrangement  of  landmarks  in 
different  types  of  environments .  41 

4.5  Application  of  directed  hypergraph  model  to  data  registration. 

(left)  Illustrated  in  the  figure  are  rigid-body  models  of  a  natural 
environment  shown  in  three  separate  coordinate  systems,  (right) 
Common  landmarks  represented  in  different  coordinate  systems  are 
determined  by  matching  directed  triangles  constructed  from  Delau¬ 
nay  triangulations  (common  directed  hyperedges  are  illustrated  by 
the  shaded  triangles).  Once  common  triangles  are  determined  be¬ 
tween  each  hypergraph,  rotation  and  translation  parameters  are 
estimated  in  closed  form  using  the  vertices  of  the  matched  triangles.  42 

4.6  Registration  of  landmarks  in  natural  and  semi-structured  environ¬ 
ments.  (a)  Rigid-body  models  of  the  Victoria  Park  are  shown  in 
the  coordinate  systems  of  a  fusion  agent  p  and  a  contributing  agent 
q.  (b)  Common  landmarks  are  determined  by  solving  a  linear  as¬ 
signment  problem  involving  directed  triangles  (the  solution  of  the 
assignment  problem  is  illustrated  in  the  figure  by  shaded  triangles 
with  corresponding  color  gradients).  The  approach  is  also  illus¬ 
trated  using  the  DLR  dataset  with  rigid-body  models  shown  in  (c) 

and  the  registration  solution  shown  in  (d) .  44 
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4.7  Data  fusion  and  likelihood  structure.  An  illustration  of  anisotropic 

data  fusion  in  the  coordinate  system  of  a  fusion  agent  p  is  shown 
above  using  the  (a)  Victoria  Park  and  (b)  DLR  examples.  The 
likelihood  matrix  A  in  each  case  provides  an  indication  of  the  un¬ 
derlying  structure  of  the  environment.  In  contrast  to  the  likelihood 
matrix  of  the  DLR  environment,  the  matrix  A  of  the  Victoria  Park 
example  naturally  exhibits  a  banded  structure .  46 

4.8  Identifying  inliers  and  outliers  in  hypergraph  registration,  (a)  Com¬ 

mon  triangles  between  the  hypergraphs  Gp  and  Gq,  determined  by 
solving  a  linear  assignment  problem,  are  indicated  in  the  figure  by 
black  markers  superimposed  on  a  likelihood  matrix  A.  (b)  Each  en¬ 
try  of  the  assignment  solution  is  used  to  compute  an  estimate  of  the 
rigid  body  parameters  t  and  9,  illustrated  above  using  a  quaternion 
representation  on  the  unit  sphere.  Inliers  (red  dots)  and  outliers 
(black  crosses)  are  determined  by  applying  random  sample  consen¬ 
sus  to  the  spherical  representation,  (c)  For  illustration  purposes, 
the  corresponding  rigid-body  parameters  (classified  as  inliers  and 
outliers)  are  plotted  in  order  of  decreasing  likelihood  statistics.  .  .  48 

5.1  Hypergraph  registration  (Victoria  Park  example):  ground  truth 

and  registration  solution.  (Ground  truth)  The  true  landmark  lo¬ 
cations  observed  by  agent  p  and  agent  q  are  indicated  by  crosses 
(+)  and  circles  (o),  respectively.  The  agents  observe  50  landmarks 
in  common  (contained  by  the  vector  /i)  with  agent  p  observing  179 
landmarks  (contained  by  up)  and  agent  q  observing  160  landmarks 
(contained  by  uq).  (Data  models)  The  individual  stochastic  maps 
of  the  agents  are  generated  using  an  additive  Gaussian  noise  model, 
with  the  map  of  agent  q  being  transformed  into  a  separate  coor¬ 
dinate  system  by  9  and  t.  (Hypergraph  registration)  Using  linear 
programming  and  outlier  rejection,  an  inlier  set  of  16  common  di¬ 
rected  triangles  (shaded  in  gray)  are  used  to  estimate  the  common 
landmark  parameters  of  the  data  models .  50 

5.2  Hypergraph  registration  (Victoria  Park  example):  likelihood  statis¬ 
tics  and  outlier  rejection.  Markers  are  used  to  indicate  entries  of  the 
likelihood  matrix  A  corresponding  to  (a)  the  true  directed  triangle 
matches,  (b)  the  triangle  matches  specified  by  the  linear  program. 

(c)  Inlier  matches  (black  circles)  are  indicated  by  the  rigid-body 
transform  parameters  that  cluster  around  the  true  values  of  t  and 
9,  with  outliers  (gray  crosses)  indicated  by  the  entries  that  fall  out¬ 
side  the  cluster,  (d)  After  applying  outlier  rejection  to  the  output 
of  the  linear  program,  16  inlier  matches  are  used  to  construct  the 
registration  solution  1Z* .  51 
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5.3  Illustration  of  overlap  and  noise  models.  Hypergraph  models  are 
shown  above  for  a  fusion  agent  p  (left)  and  a  data  contributor  q 
(right)  in  various  overlap  and  noise  scenarios.  The  top  row  illus¬ 
trates  common  triangles  (red)  and  uncommon  triangles  (white)  in 
hypergraphs  of  (a)  100%,  (b)  60%,  (c)  40%  and  (d)  0%  overlap.  The 
bottom  row  (e-h)  illustrates  the  impact  of  noise  at  a  fixed  overlap 

of  100% .  52 

5.4  Ground  truth  models  and  likelihood  structure.  The  top  row  shows 
Delaunay  triangulations  of  ground  truth  points  computed  from  (a) 

100  points  (189  triangles)  drawn  from  a  uniform  distribution,  (b) 

100  points  (185  triangles)  arranged  in  a  deterministic  grid  with  ad¬ 
ditive  noise,  (b)  the  Victoria  Park  example  with  299  points  (576 
triangles)  and  (d)  the  DLR  example  with  549  points  (1073  trian¬ 
gles).  The  bottom  row  shows  the  likelihood  matrix  A  associated 
with  each  model.  The  figure  illustrates  similarities  in  A  between  the 
natural  environment  models  of  (a)  and  (c)  and  the  semi-structured 
models  of  (b)  and  (d) .  53 

5.5  Monte  Carlo  performance  of  hypergraph  matching.  Varying  the 

levels  of  noise  and  overlap  results  in  a  change  in  (a)  the  percentage 
actual  common  triangles  relative  to  the  ground  truth  of  Gp  and 
Gq  and  (b)  the  number  of  true  positive  common  triangles  deter¬ 
mined  by  the  linear  assignment  problem  (LAP).  The  absolute  dif¬ 
ference  between  (a)  and  (b),  normalized  as  a  percentage,  is  shown 
in  (c),  which  shows  that  the  difference  between  the  LAP  solution 
and  ground  truth  is  small  for  a  reasonably  wide  range  of  overlap 
and  noise,  resulting  in  the  percentages  of  matched  triangles  after 
outlier  rejection  shown  in  (d) .  55 

5.6  Monte  Carlo  MSE  (in  dB)  performance.  The  plot  shows  that  in 

the  range  of  15%-100%  overlap  in  landmarks,  the  data  fusion  MSE 
exhibits  a  linear  degradation  in  performance  with  decreasing  SNR. 
However,  the  data  fusion  MSE  is  relatively  high  at  all  noise  levels 
below  a  15%  overlap  in  landmarks  due  to  a  general  lack  of  common 
triangles .  56 

6.1  The  author  with  several  robotic  agents.  In  the  absence  of  a  global 
frame  of  reference,  a  global  metric  model  of  an  environment  is  con¬ 
structed  from  the  data  models  of  multiple  robotic  agents  (such  as 
those  pictured  above)  by  applying  data  fusion  techniques  of  the 


nature  proposed  in  this  thesis .  58 

A.l  Block  diagram  of  sigma  point  filter .  60 
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CHAPTER  1 

INTRODUCTION 


1.1  Introduction 

The  general  nature  of  the  problem  under  consideration  is  to  construct  a  global 
map  of  landmarks  from  the  individual  efforts  of  collaborating  agents  that  operate 
outside  of  a  global  frame  of  reference  (examples  of  such  mobile  agents  are  provided 
in  Fig.  1.1).  Each  agent  independently  builds  a  data  model  of  the  environment 
referred  to  as  a  stochastic  map  [1,  2],  Constructing  a  combined  global  map  within 
a  common  reference  frame  from  the  individual  maps  of  the  agents  is  referred  to  as 
a  problem  of  data  fusion  in  multiple  coordinate  systems.  Intuitively,  the  problem 
has  the  interpretation  of  a  mathematical  jigsaw  puzzle:  the  individual  data  models 
are  the  disoriented  pieces  and  the  sought  after  global  map  is  the  completed  puzzle. 

A  benchmark  scenario  based  on  the  Victoria  Park  benchmark  dataset  is  illus¬ 
trated  in  Fig.  1.2.  The  satellite  image  shows  the  ground  truth  environment  from 
which  the  data  models  are  obtained.  Trees  (landmarks)  located  in  the  park  are 
mapped  from  the  global  reference  frame  of  the  environment  to  the  individual  local 
reference  frames  of  the  mobile  agents.  Each  agent  thus  has  an  independent,  but 
partial  model  of  the  explored  environment.  The  shared  objective  of  the  agents  is 
to  reconstruct  the  state  of  nature  from  the  sensor  measurements  independently  ob¬ 
tained  by  each  agent,  which  is  a  common  problem  encountered  in  signal  processing 
[3,  4,  5,  6,  7],  computer  vision  and  robotics. 

Stochastic  maps  are  obtained  by  independent  agents  using  various  estimation 
techniques.  In  robotics,  the  solution  to  the  simultaneous  localization  and  mapping 
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Figure  1.1:  Illustration  of  mobile  robotic  agents.  Land  rovers  such  as  (left)  Pioneer 
robots,  (center)  Segways  and  (right)  the  Mars  Curiosity  rover  are  examples  of 
mobile  agents  that  operate  in  environments  with  limited  access  to  global  reference 
frames.  The  main  topic  of  this  thesis  is  the  data  fusion  problem  of  constructing  a 
global  metric  map  from  rigid-body  data  models  obtained  in  the  reference  frames 
of  each  agent. 

(SLAM)  problem  provides  an  agent  with  a  data  model  of  the  environment  as  a 
model  of  landmark  locations  (see  [1,  2,  8,  9,  10]  and  the  references  therein).  The 
focus  of  this  thesis,  however,  is  on  the  fusion  -  rather  than  building  -  of  stochastic 
maps  (for  completeness,  however,  an  example  filtering  algorithm  used  for  building 
metric  data  models  is  provided  in  Appendix  A.l).  Our  starting  point  is  at  the 
individual  stochastic  maps,  which  are  made  available  to  a  fusion  agent  for  the 
construction  of  a  global  map. 

The  fusion  problem  with  multiple  sensor  observations  is  challenging  for  several 
reasons,  one  being  that  the  problem  contains  both  discrete  and  continuous  parts 
[11,  12],  In  order  to  construct  a  global  map,  the  fusion  agent  must  first  identify 
common  landmarks  residing  in  two  separate  maps.  Using  the  earlier  jigsaw  analogy, 
the  solver  has  to  first  identify  common  edges  in  order  to  match  the  individual  pieces. 
Prior  to  exchanging  stochastic  maps,  the  agents  are  assumed  to  operate  with  no 
prior  knowledge  concerning  the  common  landmarks  (i.e.,  the  common  trees  when 
considering  the  Victoria  Park  example)  that  are  contained  within  the  individual 
maps.  The  problem  of  matching  common  landmarks  is  of  a  combinatorial  nature 
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(data  model  p ) 


(data  model  q) 


Figure  1.2:  A  data  fusion  problem  involving  rigid-body  data  models.  The  data 
models  of  agent  p  (bottom  left)  and  agent  q  (bottom  right)  illustrate  landmark 
locations  estimated  from  the  sensor  observations  of  two  mobile  agents  operating 
outside  of  a  global  reference  frame.  Uncertainties  in  estimation  are  indicated  by 
ellipses  and  the  path  of  exploration  is  shown  by  dotted  lines.  Estimation  of  a 
common  global  map  from  the  individual  data  models,  each  obtained  in  a  separate 
coordinate  system,  requires  inferring  common  landmarks  in  addition  to  determin¬ 
ing  a  common  frame  of  reference. 


in  general,  which  eliminates  exhaustive  search  as  an  option  for  large  maps. 


Even  if  common  landmarks  between  two  maps  have  been  identified,  the  agents 
are  faced  with  the  registration  problem  of  determining  not  only  the  best  landmark 
estimates  of  common  and  uncommon  landmarks  contained  by  noisy  maps  obtained 
in  separate  coordinate  systems,  but  also  to  determine  the  spatial  parameters  of  ro¬ 
tation  and  translation.  Describing  this  again  in  terms  of  the  earlier  jigsaw  analogy: 
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not  only  are  the  pieces  disoriented,  but  the  edges  are  also  imprecise  (which  makes 
it  harder  to  see  how  the  pieces  fit  together). 

1.1.1  Related  work 

The  data  fusion  problem  considered  in  this  paper  has  been  studied  in  various 
forms.  Zhou  et  al.  [3]  derived  a  two-step  iterative  optimization  procedure  for  es¬ 
timating  the  locations  of  common  targets  using  range  and  azimuth  measurements 
obtained  by  two  separate  radar  sensors.  Each  sensor  observes  the  locations  of  tar¬ 
gets  within  a  reference  frame  related  by  a  known  displacement.  The  framework 
proposed  in  this  paper  considers  the  more  general  case  that  the  sensor  reference 
frames  are  related  by  an  unknown  displacement  and  rotation  as  part  of  a  nonlin¬ 
ear  least  squares  optimization,  the  solution  of  which  is  obtained  in  closed  form. 
Thrun  and  Liu  [11]  proposed  an  SR-tree  (Sphere/Rectangle-tree)  search  [13]  in 
consideration  of  the  matching  problem.  Common  landmark  correspondences  and 
rotation-translation  parameters  are  found  using  an  iterative  hill  climbing  approach 
to  match  triplet  combinations  formed  within  a  small  radius  of  the  landmarks  in 
each  map.  The  radius  forming  the  feature  vectors  of  the  SR-tree,  however,  would 
need  to  be  adaptive  in  order  to  generalize  to  different  environments.  Estimates  of 
common  landmarks  are  determined  separately  by  a  collapsing  operation  performed 
on  matched  landmarks  in  information  form  (see  Grime  and  Durrant- Whyte  [14], 
as  well  as  Sukkarieh  et  al.  [15],  for  further  reading  on  fusion  using  information 
filtering).  Julier  and  Uhlmann  [16]  introduced  the  covariance  intersection  algo¬ 
rithm  as  an  approach  to  the  data  fusion  problem.  Their  algorithm  uses  a  convex 
combination  of  state  information  to  achieve  data  fusion,  but  has  the  limitation 
that  the  input  data  must  be  of  the  same  dimension  (which  is  often  not  the  case  of 
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stochastic  maps  built  within  different  regions  of  exploration).  Tardos  et  al.  [17], 
and  later  Castellanos  et  al.  [18] ,  proposed  map  joining  as  a  technique  to  enable  an 
individual  mobile  robot  to  construct  a  global  stochastic  map  based  on  a  sequence 
of  local  maps.  The  approach  is  related  to  this  paper  by  considering  the  sequence 
of  local  maps  as  being  obtained  from  separate  robots,  but  requires  knowledge  of  a 
base  reference  to  construct  a  global  map. 

Williams  et  al.  [19]  considered  the  fusion  problem  by  providing  parameter  esti¬ 
mates  of  the  relative  rotation  and  translation  between  global  and  local  maps.  The 
expressions  are  derived  by  observing  the  geometry  of  the  landmarks  within  each 
map.  Our  approach  is  distinct  from  [19]  in  that  the  geometry  of  the  landmarks 
is  incorporated  in  a  nonlinear  least  squares  solution  based  on  the  maximum  like¬ 
lihood  principle.  Several  authors  such  as  Zhou  and  Roumeliotis  [20],  Andersson 
and  Nygards  [21],  Benedettelli  et  al.  [22]  and  Aragues  et  al.  [23]  considered  ren¬ 
dezvous  approaches  to  the  alignment  problem.  Rendezvous  approaches,  however, 
are  somewhat  restrictive  as  the  agents  are  required  to  be  in  close  proximity. 

The  matching  approach  of  this  paper  is  motivated  by  the  work  of  Groth  [24]  and 
Ogawa  [25].  Groth  proposed  one  of  the  earliest  matching  algorithms  in  the  con¬ 
text  of  astronomical  point  patterns,  where  a  list  of  star  measurements  are  matched 
against  a  known  star  catalog.  In  the  proposed  approach,  structured  point  triplets 
referred  to  simply  as  triangles  are  used  to  match  the  measurements  against  the  cat¬ 
alog.  The  Groth  triangle  convention  is  also  incorporated  in  our  approach,  however 
the  matching  approach  of  Groth  is  not  practical  for  large  maps  since  all  possible 
combinations  of  triangles  are  considered.  An  alternative  approach  was  proposed 
by  Ogawa  [25] ,  which  instead  incorporated  Delaunay  triangulations  [26]  to  address 
the  star  matching  problem.  This  paper  therefore  uses  Delaunay  triangulations 
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with  triangles  that  follow  the  Groth  convention  as  a  graphical  model  for  match¬ 
ing  stochastic  maps.  Further  insight  into  the  structure  of  the  model  is  found  by 
arranging  the  triangles  in  order  of  increasing  perimeter. 

1.1.2  Summary  of  results 

A  maximum  likelihood  framework  is  proposed  for  the  construction  of  a  global  map 
from  local  stochastic  maps.  The  proposed  approach  includes  1)  a  data  registration 
approach  referred  to  as  generalized  likelihood  data  fusion  and  2)  a  closed  form  least 
squares  parameter  estimation  framework  for  jointly  estimating  rotation,  translation 
and  common  landmark  parameters.  A  Gaussian  likelihood  function  is  presented 
as  the  main  proxy  for  deriving  the  procedures  of  each  step. 

Data  registration  is  a  step  that  is  performed  in  the  absence  of  a  global  frame  of 
reference,  which  requires  a  technique  that  is  affine  invariant.  To  this  end,  the  data 
models  of  each  agent  are  modeled  as  directed  hypergraphs  constructed  from  Delau¬ 
nay  triangulations.  The  hyperedges  of  each  directed  hypergraph  are  constructed 
from  directed  Delaunay  triangles  that  follow  the  Groth  convention,  which  leads  to 
an  affine  invariant  approach  for  determining  common  landmarks.  The  proposed 
registration  approach  uses  a  generalized  likelihood  ratio  as  a  matching  metric  in 
order  to  obtain  globally  optimal  landmark  correspondences  from  the  solution  of  a 
linear  assignment  problem.  The  GLR  metric  is  computed  in  closed  form  and  the 
bipartite  matching  of  hypergraphs  is  solved  in  polynomial  time  as  a  solution  to  a 
linear  programming  problem. 

Once  common  landmarks  are  identified,  the  registration  solution  of  rotation, 
translation  and  common  landmark  locations  between  the  data  models  is  computed 
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from  the  vertices  of  the  hypergraph  model.  The  main  contributions  include  1) 
a  closed-form  solution  to  the  parameter  estimation  problem  of  determining  rigid- 
body  transform  parameters  as  the  solution  to  a  nonlinear  non-convex  optimization, 
2)  benchmark  performance  bounds  obtained  in  closed  form  and  a  quantitative 
metric  for  determining  the  value  of  data  fusion  (which  has  application  to  decision 
making  problems  that  may  arise  in  the  area  of  data  fusion)  and  3)  a  robust  data 
registration  approach  for  obtaining  for  common  directed  triangles  in  presence  of 
many  outliers. 


1.2  Organization 

The  remainder  of  this  thesis  is  organized  as  follows.  Chapter  2  introduces  the 
estimation  models  used  to  combine  data  contributed  by  a  fusion  agent  and  a  con¬ 
tributing  agent,  each  of  which  obtain  the  data  is  separate  coordinate  systems. 
Chapter  3  derives  benchmark  performance  bounds  the  proposed  estimators  and 
includes  an  analytical  study  on  the  value  of  data  fusion.  Chapter  4  details  a  data 
registration  approach  based  on  the  generalized  likelihood  statistics  of  a  hypergraph 
model  constructed  from  the  data  of  each  agent.  Concluding  remarks  are  provided 
in  Chapter  6. 
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CHAPTER  2 

MAXIMUM  LIKELIHOOD  ESTIMATION 

2.1  Environment  model  and  available  data 

A  fusion  agent  p  is  an  agent  that  seeks  to  build  a  global  metric  model  of  an 
environment  from  data  exchanged  with  a  contributing  agent  q.  A  summary  of  the 
parameters  used  to  describe  the  data  fusion  problem  is  as  follows. 

1.  Common  landmarks:  The  parameter  p  contains  the  ground  truth  locations 
of  n  landmark  locations  estimated  by  both  a  fusion  agent  p  and  a  data 
contributor  q. 

2.  Uncommon  landmarks:  The  parameters  vp  and  vq  contain  the  ground  truth 
locations  of  landmark  locations  estimated  by  the  fusion  agent  p  and  the 
contributor  q,  respectively,  without  any  overlap  on  landmark  locations. 

3.  Rigid-body  transformation:  The  rotation  parameter  9  and  the  translation  t 
define  a  rigid-body  transform  between  the  coordinate  systems  of  the  fusion 
agent  and  the  contributor. 

4.  Data:  Landmark  locations  estimated  in  the  coordinate  systems  of  the  fusion 
agent  p  and  the  data  contributor  q  are  contained  by  the  data  vectors  xp  and 
xq,  respectively.  The  covariances  Ep  and  Eg  model  the  uncertainty  in  the 
data  of  agent  p  and  agent  q ,  respectively. 

Without  loss  of  generality,  the  coordinate  system  of  the  fusion  agent  p  is  defined 
in  the  ground  truth  coordinate  system  containing  the  parameters  {p,  vp,vq}. 
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Some  notations  used  in  this  chapter  are  as  follows.  The  symbol  <E>  denotes  the 
Kronecker  product  operator  (i.e.,  A®  B  =  [a.ijB])  and  the  matrix  Im  is  an  m  x  m 
identity  matrix.  The  matrix  r(6)  G  M2x2  denotes  a  rotation  matrix 


r(9)  = 


(  a  ■  a\ 
cos  a  —  sm  9 


sin  9  cos  6 


(2.1) 


with  rotation  parameter  6  G  [— ir,  7r],  with  a  block  diagonal  form  given  by  R(9)  = 
Im  <E>  r{6).  An  equivalent  form  of  (2.1)  is  given  by  r{6)  =  Iccos9  +  IssmO,  where 
the  constant  2x2  matrices  Ic  and  Is  are  defined  as 


and 


-1 

0 


respectively,  with  nonzero  entries  corresponding  to  the  cosine  and  sine  functions 
of  the  rotation  matrix  r(9).  A  translation  input  matrix  is  a  matrix  of  the  form 
F  =  em  ®  I2,  where  em  is  an  m- vector  with  all  entries  equal  to  1. 
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2.2  Gaussian  model  of  metric  maps 


Let  the  random  vectors  Xp  and  Xq  represent  the  noisy  observations  obtained  by 
agents  p  and  q,  respectively.  Prior  to  fusion,  data  is  collected  in  the  separate 
coordinate  systems  of  the  agents  (i.e.,  Xp  and  Xq  reside  in  coordinate  systems  p 
and  q,  respectively).  The  statistical  model  of  matched  Gaussian  maps  is  given  as 


Xp  Up  T  Wp 


(2.2) 


Xq  =  R(9)uq  +  Ft  +  Wq 


(2.3) 


where  Wp  ~  A/”(0,  apI )  and  Wq  ~  A/”(0,  aqI)  are  independent  zero-mean  ad¬ 
ditive  Gaussian  noise  vectors.  The  ground  truth  observed  by  agent  p  is  then 
Up  =  ( pT,vp)T ,  which  is  defined  in  the  coordinate  system  of  agent  p ,  and  the 
ground  truth  of  agent  q  is  uq  =  (pT,iX)T  in  the  global  reference  frame,  which  is 
observed  in  coordinate  system  q  as 


R(6)uq  +  Ft 


Ri{0) 


\/..\  tF\ 


R,m 


p 

W 


+ 


t 


\F°/ 


(2.4) 


where  the  subscripts  1  and  0  indicate  the  partition  the  map  into  common  and 
uncommon  parts. 


In  order  to  separate  the  data  registration  and  parameter  estimation  problems, 
an  assumption  is  made  that  the  common  landmarks  in  both  maps  are  known.  The 
process  of  obtaining  such  a  matching,  however,  is  nontrivial  and  combinatorial  in 
general  (see  Section  4.3  for  an  affine  invariant  procedure  for  determining  common 
landmarks) . 
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2.3  Likelihood  decomposition  and  closed  form  solution 

Estimators  of  the  parameters  {/q  vp,  vq,  t,  9}  are  derived  by  considering  the  likeli¬ 
hood  function  of  the  combined  global  map  given  by 

L(n,vp,vq,t,0)  =  riexp~J(fi,vp,vq,t,d)  (2.5) 

where  the  function  J  is  dehned  as 

J(n,vp,vq,t,9 )  =  \\\xp  -  UpW1  +  \\\xq  -  R(9)uq  -  Ft\\2  (2.6) 

Vp  Vq 

with  xp  =  ( XpT ,  XpT)T  and  xq  =  {x1^ ,  %qT)T,  corresponding  to  the  structure  of  up 
and  uq,  respectively.  By  partitioning  the  problem  into  common  and  uncommon 
parts,  it  immediately  follows  that  (2.6)  decomposes  as 

J (/b  Vpi  Vqi  t,  0)  J0(Vp,Vq,t,d)  “h  t/l(/i,f,0)  (2.7) 

where  J0  is  the  squared  error  function  of  estimating  the  uncommon  landmarks  vp 
and  vq,  including  the  transform  parameters  {t,9},  specihed  as 

J0(vp,Vq,t,9 )  =  \\\xl  -  Vp\\2  +  ^\\x°  -  R0(9)vq  -  F0t\\2  (2.8) 

( ^  (J z  * 

^  p  q 

and  J\  is  the  squared  error  function  of  estimating  the  common  landmarks  contained 
by  the  vector  /i,  also  including  {t,9},  specihed  as 


Ji(n,t,9)  = 


hH2  + 


RMn-Fit  ||2. 


(2.9) 
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This  decomposition  is  exploited  to  minimize  the  combined  error  function  J  by 
minimizing  J0  and  ,J\  separately,  as  stated  by  the  following  lemma. 

Lemma  1  (Separable  optimization)  Let  {/x*,v*,v*,t*,9*}  be  the  global  maxi¬ 
mum  of  the  likelihood  function  L,  i.e., 


J{n*,v*v*t*X) 


min  J(n,vp,vq,t,9). 

fl,Vp,Vq,t,0 


(2.10) 


If  the  solution  {fi,t,9}  is  the  global  minimum  of  J\  given  by 

( p,,t,9 )  =  argmin  t,  9),  (2-11) 

then  fi*  —  fi,  t*  —  t.  9*  =  9  and 

v;  =  x%  v;  =  R^0)(x°q  -  F0i)  (2.12) 

respectively. 

Proof:  With  the  decomposition  J  =  Jo  +  J\ ,  the  proof  is  immediate  by  noting 
that 

J0(x°p,RT(9)(x°q-F0t),t,9)  =  0  (2.13) 

for  any  {/i,  t,  9}.  □ 


Lemma  1  shows  that  the  maximum  likelihood  solution  of  the  combined  map 
specified  by  u*  =  (/i*T,  v*T,  v*T)T  is  obtained  from  the  nonlinear  least  squares 
optimization  of  the  non-convex  function  J\.  A  global  minimum  is  obtained  by 
deriving  an  equivalent  expression  of  J\  as  a  sinusoidal  form  parameterized  by  the 
unknown  rotation  parameter  9 ,  which  leads  to  a  closed  form  solution  as  stated  by 
the  following  theorem. 
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Theorem  1  (Closed  form  MLE)  The  ML  estimators  of  the  parameters 
{/x,  t,  9}  are  given  by  the  following  expressions. 

1.  The  MLE  of  the  rotation  parameter  9  is 


9*  =  sgn(/3) 


cos 


-1 


a 


+  P2 


7T 


(2.14) 


where  sgn(-)  is  the  signum  function.  The  coefficients  a  and  (3  are  given  by 


a  =  -XgT(In  ®  Ic)QXp  (2.15) 

P  =  -xjr(/n  <g>  I9)Qx1p  (2.16) 

respectively,  where  Q  =  L2n  —  F\(Ff  Ff)~x  Ff  with  n  being  the  number  of 
common  landmarks. 

2.  The  MLE  of  the  translation  t  is 

<*(#•)  =  (Jf  [x]  -  R(e-)x'p]  (2.17) 

denoted  hereafter  as  t* . 

3.  The  MLE  of  the  common  landmarks  p  is 

pf  (9*)  =  </>yp  +  <j>*qx\  (2.18) 


denoted  hereafter  as  p* .  The  matrix  gains  0*  and  0*  are  given  by 


respectively. 


=  L 


at. 


2  n 


Q 


at 


(J2  +  (T2 
^  p  1  ^  q 


QRT  (9* 


(2.19) 

(2.20) 
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The  following  lemmas  are  used  in  the  proof  of  Theorem  1. 

Lemma  2  The  matrix  Q  is  an  idempotent  and  symmetric  matrix  that  commutes 
with  a  block  diagonal  matrix  of  the  form  A  =  /„§  B ,  with  B  e  M2x2. 

Proof:  Using  the  expression  for  FTF  given  by 

FtF  =  [en  ®  I2]T[en  <g>  I2\ 

=  [e£e„]  <8>  [If  h\ 

=  nl2 

it  follows  that  (FTF)_1  =  ^/2  and  F(FTF)_1FT  =  pLFFT  where 

FFT  =  [en  ®  I2]  [en  ®  I2]T 
=  [eneTn]®[hlf] 

= 

=  (FFTf 

meaning  pl  FFT  =  F(FTF)~1F1  =  Q  is  symmetric,  leading  to  the  conclusion  that 

QT  =  c hn-Q)T 

=  II.  -  0T 
l2n  Q 
=  Q 

which  proves  that  Q  is  symmetric.  Notice  that  the  product  of  Q  with  itself  is 


14 


29 


QQ  =  {hn-Q){hn-Q) 

=  I2n  ~  2 Q  +  QQ 
=  l2n  2  Q  +  Q 
1 2- n  Q 
=  Q 

which  proves  that  Q  is  idempotent.  The  product  of  the  matrix  FFT  and  the  block 

diagonal  matrix  A  =  In  <£>  B  is  given  by 

FFtA  =  \(en®  I2){en®  I2)T][In®  B\ 

=  [(e„e^)  (8)  I2\  [In  ®  B\ 

=  [{eneTn)In]®[l2B} 

=  [In(eneTn)]®[BI2] 

=  [In  ®  B]  \(enel)  ®  I2\ 

=  [In  ®  B]  [(en  ®  I2)(en  ®  I2)T] 

=  AFFt. 

Since  FFT A  =  AFFT  and  FTF  =  nl2 ,  it  follows  that 

QA  =  [I2n  —  F(FtF)~1Ft]A 

=  a~iffta 

n 

=  a~iafft 

n 

=  A[I2n  —  F(FtF)~1Ft] 

=  AQ 

which  proves  that  Q  and  A  commute.  □ 
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Lemma  3  The  sinusoidal  form  with  constant  coefficients  a,/3, 7  G  M.  defined  as 


Js{9)  =  a  cos  9  +  fisinO  +  7,  9  G  [— n,  7r]  (2.21) 

reaches  a  global  minimum  at  the  value  9*  =  T (j3,a),  where  the  function  T  is 
defined  as 


T  (P,a)  =  sgn(/3) 


cos 


-1 


a 


+  /52 


—  7T 


(2.22) 


where  sgn(-)  is  the  signum  function. 


Proof:  The  following  proof  is  elementary  in  nature  and  applies  for  all  7  G  R. 
since  7  is  constant  with  respect  to  9.  Consider  the  trigonometric  identity  of  the 
form 


(k  cos  ip)  cos  9  +  (n,  sin  ip)  sin  9  =  n  cos  ( 9  —  ip) 


(2.23) 


with  kg!  and  9,  p  G  [— tt,  tx\.  By  considering  the  sum 


a2  +  fi2 


(kcos</?)2  +  (fcsin<^)2 
k2  (cos2  p  +  sin2  ip) 


where  a  =  ncosp  and  ft  =  k  sin  p,  it  follows  by  taking  square  roots  that  k  = 
\J a2  +  fi2  so  that 

a  cos  9  +  (3  sin  9  =  \J a2  +  /32  cos  ( 9  —  p)  (2.24) 


16 


31 


with  a,j9eK.  An  expression  for  the  angle  p  is  found  by  considering  the  ratio 


f3  k  sin  (p 

a  k  cos  p 

=  tan  p 

which  leads  to  the  relation 


(2.25) 


The  arctangent  in  (2.25)  is  typically  implemented  using  the  atan2  function,  which 
is  expressed  in  closed  form  as 


«)  =  sgn(/3)  cos 


a 

\joi2  +  /32 


(2.26) 


as  a  function  of  a  and  f3  (note  that  the  expression  is  undefined  in  the  case  that 
a  —  —  0).  ft  directly  follows  that  the  sinusoidal  form  is  maximized  by  the  value 

Omax  =  p(/3,  a)  and  minimized  by  the  value 


e*  =  T  (P,a) 
=  sgn(^) 


cos 


-1 


a 


y/tt2  +  /32 


—  7T 


by  observing  that 


a  cos  0*  +  f3  sin  6* 


k  cos  ( 9 *  —  p(/3,  a)) 
hi  cos  (—  sgn(/3)7r) 
hi  cos  (tt) 


where  cos  (7r)  =  —  1  is  the  minimum  value  of  cos#  on  the  interval  6  G  [— n,  7r].  □ 
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Proof:  [Proof  of  Theorem  1]  Minimizing  (2.38)  with  respect  to  (w.r.t.)  fi  leads 


to 


MM)  = 


°l+°2q 


4 Xp  +  -4 RT(0)(xq  -  Ft) 

G  G 

V  Q 


(2.27) 


and  minimizing  (2.38)  w.r.t.  t  leads  to 


t(d,/i)  =  (F  F)  F  (xq-R(6)fj,). 


(2.28) 


Using  the  evaluation  /i  =  n{9,t)  in  (2.28)  results  in  the  MLE  of  t  as  a  function  of 
6  given  by 


t*(9)  =  (FtF)~1Ft  (xq  -  R(9)xp) .  (2.29) 


Applying  the  evaluation  t  =  t*{6)  in  (2.27)  leads  to  the  MLE  of  fi  as  a  function  of 
9  given  by 


P*{9)  =  4>p(6)xp  +  4>q{9)xq. 


(2.30) 


Using  the  symmetry  and  idempotence  properties  of  the  matrix  Q  (Lemma  2),  it 
follows  from  the  expression  (2.30)  that 

\\xp  -  Id*(6)\\2  =  Kp\\xq  -  R{9)xvW2qtq 
|  \%q  R{0)Xp\\Q 

where  kp  =  42<+<r2)  ■  Similarly,  it  follows  from  (2.29)  and  (2.30)  that 


xq-R{9)n*{9)-Ft*{9) ||-  =  Kq\\xq-  R(6)xp\\2QTQ 
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(a2  \  2 

where  Kq  =  (  - 2^2  )  ■  Using  these  simplifications  to  define 


■W)  =  ±Ji  <ji*(0),rW),o) 

=  l\\Xq  ~  R(0)Xp\\q 


(2.31) 


where  k  =  -^Hp  +  ~2^q  and  expanding  the  norm  in  the  right  hand  side  (RHS)  of 
(2.31)  as 


\xq  -  R{6)xp\\2q  = 


XpRT(9)QR(9)xp  +  xqQxq 
-2  x^QR(9)xp, 


(2.32) 


it  follows  from  Lemma  2  that  the  first  term  on  the  RHS  of  (2.32)  reduces  to 


xTvRT(9)QR(9)xp  =  xTpRT{9)R{9)Qx 


=  XpQxp 


so  that  from  (2.31),  (2.32)  and  (2.33),  </*(#)  reduces  to 


(2.33) 


J{{9)  =  2(xIQxp  +  xTqQxq  ~  2 xTqR(9)Qxp). 


(2.34) 


Notice  in  the  last  term  on  the  RHS  of  (2.34)  that 


xqR(9)Qxp  =  xq  [Rc{9)  +  Rs(9)}  Qxp 

=  xqRc(9)Qxp  +  XqRs(9)Qxp 


(2.35) 


where  Rc{9)  =  (In  <g)  Ic)  cos(0)  and  Rs[9)  =  (In  <g)  Is)  sin(6)),  meaning  that 


—2 xqR(9)Qxp  =  2a  cos (9)  +  2/3sin(d) 
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where  a  =  —x^{In® Ic)Qxp  and  f3  =  —x^(In^Is)Qxp,  from  which  it  immediately 
follows  that 


J{(9)  =  acos(d)  +  /3sin(d)  +7  (2.36) 

where  7  =  \  (x^  Qxp  +  x^Qxg).  By  virtue  of  the  sinusoidal  form  (2.36),  it  follows 
from  Lemma  3  that  J{{9)  has  a  unique  minimum  for  6  e  [— n,  n\  given  by  (2.14), 
which  leads  to  the  MLEs  of  t  and  p  given  by  (2.17)  and  (2.18),  respectively.  □ 

Theorem  1  provides  a  closed  form  solution  for  maximizing  the  likelihood  L\ 
of  common  landmarks  in  the  case  that  the  covariances  of  p  and  q  are  of  the  form 
E p  =  crpl2n  and  Eg  =  cr^^n,  respectively,  and  the  data  partitioning  into  common 
and  uncommon  parts  is  known.  Specifically,  given  the  likelihood 

LMt,0)  =77 exp  -^.Ji(n,t,9)  (2.37) 

where  the  function  J\  is  dehned  as 

Ji(n,t,9)  =  \\xp  -  Hls-i  +  \\x\  ~  R{9)p  -  (2.38) 

Theorem  1  specifies  the  parameters  p*,  t*  and  6*  such  that 

Li(p* ,t* ,9*)  —  maxi^/i,  t,  9)  (2.39) 

fj,  ,t,6 

for  Ep  =  <Tp/2n  and  Eg  =  a^I2n.  An  important  note,  however,  is  that  Q  =  02X2  when 
n  =  1,  meaning  n  >  1  common  landmarks  are  required  to  compute  the  solution  of 
Theorem  1  (a  minimum  of  n  =  3  common  landmarks  are  recommended). 
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2.4  Anisotropic  approximation 

An  extension  of  the  ML  fusion  rule  (2.18)  is  derived  by  considering  covariances 
and  T,q  of  anisotropic  structure  (i.e.,  no  assumption  on  structure  beyond  the 
requirements  of  being  positive  definite  and  symmetric).  In  this  case,  least  squares 
estimators  satisfying  (2.39)  are  no  longer  found  in  closed  form  due  to  the  nonlinear 
parameter  9.  Using  the  closed  form  solution  6*  provided  by  Theorem  1,  least 
squares  estimators  of  the  parameters  yU  and  t  are  derived  instead  to  satisfy 

Li(p,,i,9*)  —  maxLi(n,  t,  9*)  (2.40) 

fl,t 

resulting  in  a  closed  form  approximation  of  the  parameter  yU  of  the  form 

ft(9*)  =  %(9*)x1p  +  <S>q(9*)x1q  (2.41) 

where  the  fusion  gains  and  are  given  by  Algorithm  1  (see  Section  2.4.1  for 
derivation).  By  deriving  ft  as  a  a  linear  combination  of  x *  and  xq,  a  combined 
covariance  matrix  is  then  specified  as 

£{o*)  =  $P(r)sp[$p(r)]T  +  <s>q{9*)zq[$q(9*)}T.  (2.42) 

An  illustration  of  the  anisotropic  approximation  is  shown  in  Fig.  2.1  in  a  data 
fusion  scenario  involving  n  —  3  common  landmarks  (the  performance  of  the  pro¬ 
posed  fusion  rules  is  discussed  in  Section  5.2).  Some  properties  of  the  matrix  gains 
and  $5  are  provided  in  Section  2.4.2. 
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-20  -10  0  10  20 

y  [m] 


Figure  2.1:  Illustration  of  anisotropic  data  fusion:  (top)  x  and  (bottom)  y  loca¬ 
tions.  Three  landmarks  (vertical  black  lines)  located  at  the  coordinates  (—15,  —15), 
(0,  0)  and  (15, 15)  are  estimated  individually  by  a  fusion  agent  (bold  lines)  and  a 
contributing  agent  (dashed  lines).  Data  fusion  enables  the  fusion  agent  to  esti¬ 
mate  the  location  of  landmarks  by  incorporating  the  data  of  a  contributing  agent 
(illustrated  in  red).  The  peak  of  each  distribution  corresponds  to  the  estimated 
landmark  location,  while  the  width  indicates  the  uncertainty  of  the  estimate. 
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Algorithm  1  Computation  of  <f>p(d)  and  &q(9) 

Input:  Relative  rotation  9  e  [— n,  n\  and  covariance  matrices  e  M2nx2n  (agent 
p)  and  Tjq  G  M2nx2n  (agent  q) 

Output:  $p(0)  and  &q(0) 

Application:  Multi-coordinate  system  data  fusion 
1:  R{6)  =  In®r{6) 

2:  S(#)=[Ep  +  Jir(#)E-1fl(e)]-1 
3:  m  =  E-1  -  E-'i?(#)S(#)flT(e)E-> 

4:  F  =  en®  I2 

5:  Q(6)  =  F(FTn(6)F)~1FT 

6:  £,(0)  =  S,  +  R{9)^PRT{9)  -  Q(0) 

7:  W(9)  =  Y.{9)RT{9)Y,~1 

8:  $p(0)  =  [s(0)  +  1G(0)Q(0)1GT(0)]  S;1 

9:  $,(0)  =  W (9)Eq(9)WT(9)E~1RT(9) 

2.4.1  Derivation  of  fusion  gains  4>p{6)  and  §q{6) 

Given  the  parameter  9,  the  likelihood  equations  associated  with  the  unknown 
parameters  ft  and  t  are 

=0  and  gktAiM)=0  (2,43) 

dfi  dt  K  J 

respectively.  The  estimators  of  /i  and  t  that  satisfy  the  respective  likelihood  equa¬ 
tions  are 

/i  =  S(0)  +  RT (xq  -  Ft)]  (2.44) 

where  £(0)  =  [E;1  +  RT {9)E~1  R(9)] _1  and 
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i  =  (FTS-1F)-1FTi:-1  (xq  -  R(9)ii)  (2.45) 

respectively.  Using  the  expression  (2.44)  and  the  evaluation  n  —  p,  in  (2.45)  leads 
to  the  expression 

i  =  {FTn{e)F)-1FTE~1  {-R(9)t,(9)T,~1xp  +  [I2n  -  R(9)t(9)RT(9) E-1]  xq) 

(2.46) 

where  Q(9)  =  S”1  —  T,~1R(9)Ti(9)Rt(9)T,~1.  Using  the  expression  (2.46)  and  the 
evaluation  t  —  t  in  (2.44)  leads  to  the  expression 

m  =  S(0)  {[s;1  +  i?T(d)A'(d)]a;p  +  ^(^[S-1  -  A^)AT(d)K}  (2.47) 

where  K{9)  =  E-1Q(9)E-1R(9)E(9)E~1  with  Q(9)  =  F(FTn(9)F)~1FT .  It  fol¬ 
lows  that 

m  =  V  p(9)xp  +  Tq(9)xq  (2.48) 

where  the  matrix  Tp(d)  is  given  by 


rPm  =  m  [s;1  +  «r(«)/f(9)] 


=  s(o)s-‘  +  smRTmE-iQms-iR(n)sms 


-i 


E(9)  +  E(9)Rt(9)S-1Q(9)E-1R(9)E(9) 


q 

T  t 


V-1 
^ P 


=  ^(0)  +  W{9)Q(9)WT(9) 

= 


y-i 
^ p 
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where  W(0)  =  S(0)i?T(0)S“1  and  %(6)  =  |e(0)  +  W(9)Q(9)WT(9)  1  E;1  and  the 
matrix  rg(0)  is  given  by 

r,(<?)  =  ne)RT{0)  [e-1  -  k(9)rt(9)] 

=  h(9)rt(9 )  [e-1  -  ^Qm^Rmm^R^o) 

=  t(9)RT(9) E-1  [j2n  -  <5(^)E“1-R(0)S(6*)E“1i?T(6*)' 

=  E(0)i2T(0)E"1  [^Ep^E^i^E,-*^)]  E“1i?(0)E(0)E“1.RT(0) 

=  W(0)  [fl^Ep^E-^i^Eg-Q^)]  Wt{9)Z~1Rt{9) 

=  W(0)  {i2(0)Ep(0)  [E”1  +  Rt(9)E~1R(9)]  Rt(9) E,  -  Q(0)}  WT (9)^ RT (9) 
=  W(9)  [Eg  +  R(9)TjpRt (9)  -  Q(0)]  W"T(d)E;1i?T(0) 

=  bP  (0)Eg(0)VPT(0)E“1.RT(0) 

=  *,(<?) 

where  £,(0)  =  Eq  +  i?(0)Epi?T(0)  —  Q(6)  and  <h?(0)  =  W(9)tq(9)WT(9)Y>~lRT(9). 

2.4.2  Properties  of  $p(0)  and  4>g($) 

The  following  properties  of  <hp(0)  and  $5(0)  are  considered. 

1.  Property  1  states  that  the  anisotropic  approximation  provided  by  (2.41) 
yields  the  closed  form  solution  (2.18)  in  the  case  that  the  Ep  and  Eg  are 
isotropic  in  structure. 

2.  Given  that  the  data  provided  by  each  agent  is  obtained  in  separate  coordinate 
systems,  it  follows  that  a  single  landmark  does  not  provide  sufficient  context 
for  constructing  a  combined  map.  Property  2  thus  states  that  in  the  case  of 
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a  single  common  landmark,  the  fusion  agent  p  will  retain  its  own  landmark 
estimate  and  ignore  the  data  provided  by  the  contributor  q.  A  meaningful 
application  of  data  fusion  in  multiple  coordinate  systems  therefore  requires 
the  data  to  contain  n  >  1  common  landmarks. 

3.  In  the  case  that  the  coordinate  systems  of  the  agents  are  the  same  (and  in 
fact,  for  arbitrary  t),  Property  3  states  that  the  data  fusion  rules  provided 
by  (2.18)  and  (2.41)  yield  an  affine  combination  of  the  common  landmarks 
Xp  and  xxq.  In  the  general  case  of  multiple  coordinate  systems,  however,  the 
proposed  fusion  rules  specify  a  linear  combination  of  the  data  provided  by 
each  agent. 


Property  1  If  Sp  =  <Tp/2n  and  =  o-qI2n,  respectively,  then  &p(9*)  =  and 

*,(#•)  =  <K- 


Proof:  Using  the  simplifications  of  £(#),  Q{6)  and  W(0)  given  by 


m 

Q{0) 

W(9) 


( 7 2  +  (J - 
^  r)  1  ^  n 


<-2n 


(h  +  k)Q 

r2 


ol  +  o': 


-Rt(6) 


respectively,  where  Q  =  F(FTF)  lFT ,  it  follows  that 


4>p(0)  =  £(0)  +  W(6)Q(6)WT(d) 


y-l 
^ P 


_ 

hn  + 

0-2+0-2 

to  — 

al 

-*2  n 

cr|+cr2 

TbQ 


Q 
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Since  E(0)  =  (a2  +  cr2)Q  when  Ep  =  a2I2n  and  Eq  =  cr2I2n,  the  matrix  $q(9) 
reduces  to 


$,(0)  =  w(6)tq(e)wTm^RT(0) 

=  ^ QRT(o ) 

so  that  %(9)  =  I2n  -  and  ®q(°)  =  QRT(9). 

Property  2  If  n  —  1,  then  <3>p(0)  =  I2  and  &q(9)  =  02x2  for  9  e  [ — 7r,  7r] . 


□ 


Proof:  Since  Q(9)  =  T,q  +  R(9)Y,pRT (9)  when  n  —  1,  it  follows  that 

tg(9)  =  Eg  +  R(6)i:pRT(6)  -  Q{9) 

=  Eg  +  R(6)ZpRt(6)  -  [E,  +  R(9)ZpRt(6)] 

=  02x2 


so  that  <I>p(#)  reduces  as 


Z(9)  +  W(9)Q(6)Wt(9) 


=  [£(«)  +  sP  -£(«)] 


=  h- 


Since  £g(0)  =  02X2  when  n  =  1,  $q(9)  reduces  as 


$g(0)  =  W(9)£q(9)WT(9)Z~1RT(9) 
=  W(9)[02x2]Wt(9)Z-1Rt(9) 
=  02X2 


meaning  <£>p(d)  =  I2  and  ®q(9)  =  02x2  when  n  —  1. 


□ 
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Property  3  If  9  =  0,  <£>p(0)  +  $g(0)  =  I2n. 

Proof:  Using  $>p(9)  and  <f>g(d)  provided  in  Algorithm  1  and  the  equivalent 

form  of  E q{9)  when  6  =  0  given  by 

Eg(0)  =  Eg  +  i?(0)Epi?T(0)  —  Q(0) 

—  Eg  +  Ep  —  Q(0) 

it  follows  that  <£>p(0)  and  <3>g(0)  are  then 


«v(°) 


E(0)  +  H/(0)Q(0)H/T(0) 


and 


^(0) 


W  (0)Eg(0)fUT(0)E“1i?T(0) 
W(0)  [Eg  +  Ep-g(0)l  1UT(0)E 


-1 

p 


respectively.  The  sum  of  ‘hp(O)  and  $5(0)  is  then 


■MO)  +  4,(0)  =  2(0)2;'  +  W( 0)  (2P  +  2,)  H-'T(0)2;' 

=  [2(0)  +  W( 0)  (2,  +  2,)  H-'T(0)]  2;1. 

However,  since  W( 0)  =  E(0)i?r(0)Eg  1  =  E(0)E“1  when  9  =  0,  it  follows  that 

4„(0)  +  4,(0)  =  [2(0)  +  W( 0)  (2P  +  2,)  H-'T(0)]  2;' 

=  [2(0) +  2,- 2(0)]  2;' 

=  PP]S;' 

—  hn 


which  proves  that  $p(0)  +  $g(0)  =  I2n. 


□ 
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CHAPTER  3 

CRAMER- RAO  BOUNDS  OF  MODEL  PARAMETERS 


3.1  Introduction 


This  chapter  derives  benchmark  performance  bounds  known  as  the  Cramer-Rao 
lower  bounds  [27,  28,  29,  30]  for  the  maximum  likelihood  estimators  proposed  in 
Theorem  1.  The  bounds  are  derived  by  applying  the  following  theorem  in  the  case 
of  Gaussian  model. 

Theorem  2  Suppose  that  a  random  variable  X  has  an  n-variate  Gaussian  distri¬ 
bution  with  a  probability  density  function  parameterized  by  0  as 


Px(x ;  0)  =  — .  =  exp  — \x  —  m(0)]TE_1(0)ta  —  m(0)l, 

1  ’  >/(2tt)"|E(0)|  2 


(3.1) 


then  the  entries  of  the  Fisher  information  matrix  I (0)  are  given  by 


hM)  = 


dm((j)) 

defi 


S-A(0) 


+  ^  tracers  x(0) 


lT 


S"1(0) 


dm((j)) 

<907 


(3.2) 


so  that  I (0)  =  [I, 


Proof:  See  Kay  [31],  App.  3C. 


□ 
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3.2  Gaussian  Cramer-Rao  lower  bounds 

Consider  the  Gaussian  model  of  n  points  (referred  to  as  landmarks)  given  by  the 
random  variable  of  the  form 


x~A/-(ro(0),E(0)) 


(3.3) 


with  model  parameters  0  =  (pT  ,tT  ,6T)T  consisting  of  the  (x,  ^-coordinates 
p  G  M2n,  the  rotation  angle  9  G  [— n,  7r]  and  the  translation  parameter  t  G  M2. 
The  deterministic,  but  unknown,  mean  of  (3.3)  is  partitioned  into  two  parts,  cor¬ 
responding  to  coordinates  systems  p  and  q ,  as 


m{4>) 


(  \ 

^  R(6)p  +  Ft  ^ 


(3.4) 


with  covariance  E(0)  =  cr2/4n.  Under  this  model,  the  entries  of  the  Fisher  infor¬ 
mation  matrix  [31]  1(f)  =  [/*.,•  (0)]  are  given  by 


1 

dm(4>) 

T 

dm  (0) 

a2 

dfi 

.  d(t>j  . 

(3.5) 


which  are  computed  from  the  partial  derivatives  of  m(0)  with  respect  to  the  model 
parameters.  In  vector  form,  the  partial  derivatives  are  given  by 


D, 

Dt 

De 


dm  (0) 
dp 


dm  (0) 
dt 


dm  (0) 
d6 


(  r  \ 

J-2n 

\  m ) 

/„  \ 

U2nx2 


V 


) 


(  o  \ 

U2nxl 


(  R(S)Lp  ) 


(3.6) 

(3.7) 

(3.8) 
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with  Ls  =  In 


0  -1 

1  0 


\ 


,  which  leads  to  the  Fisher  information  matrix 


m  =  - 


<7 


<7 


DT,D, 

DlDt 

dJdC 

DJD, 

DfDt 

DJD, 

DTeD, 

DTeDt 

DfDg  j 

2 12n 

R1 

\0)F 

LTspi 

FtR{6 )  FTF 

FT  R(9)Lf  pi 

pTLs 

l-iTLs 

Rt(8)F 

piT  pi 

\ 


(3.9) 


Using  the  matrix  (3.9),  the  Cramer- Rao  lower  bound  (CRLB)  covariance  of  the 
model  parameter  <f>  is  then  P{<f>)  =  /_1(0),  which  leads  to  the  following  theorem 
concerning  the  performance  bounds  of  the  entries  of  <fi. 


Theorem  3  The  Cramer-Rao  lower  bounds  (CRLBs)  of  the  parameters  {/i,  t,9} 
are  given  by  the  following  expressions. 


1.  The  CRLB  of  the  rotation  parameter  6  is 

Pep  =  o-2^  (3.10) 

where  (  =  and  Q  =  I2n  —  F(FT F)~lFT . 

2.  The  CRLB  of  the  parameter  pi  is 

U/t,,i  =  (T2  (u  +  (3.H) 


where  U  =  I2n  —  \Q  and  u  =  \QLfp.. 
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3.  The  CRLB  of  the  translation  t  is 


Pt,t  =  cr2  [V  +  -vv1 


(3.12) 


where  V  =  |/2  and  v  =  ^FT  R(6)Lf  /a. 

Proof:  The  CRLBs  of  the  model  parameters  {/i,  t,  6}  are  determined  by  con¬ 
sidering  the  block  partitioning  of  the  Fisher  information  matrix  (3.9)  of  the  form 


m  =  f 


l  A 

\bT  c  j 


(3.13) 


where  A,  b  and  c  are  given  by 


A  = 


1  2 12n  Rt{9)F  ^ 

y  FtR(6 )  FtF  ) 


,  b  = 


K  FTR(d)L^  } 


and  c  =  /j,t ft,  respectively.  The  inverse  Fisher  information  matrix  I  l((f)  is  com¬ 
puted  from  the  inverse  of  A,  which  is  determined  by  using  the  matrix 


/ 


B  = 


L2n 


0 


\ 


2nx2 


(3.14) 


-i  FtR(0)  h  ) 


to  decompose  A  into  a  block  diagonal  form  (known  as  the  Aitken  block- 
diagonalization )  given  by 


A  =  BABt 


( 


21 


\ 


2  n 


\ftf 


(3.15) 
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Using  the  relation  FTF  =  71/2,  it  follows  that  the  inverse  of  the  matrix  A  is 
computed  as 


A-1  =  (B-1!  [B7]"1)- 
=  BtA~1B 

(  U  -±Rt(0)F^ 
FTR(0)  V  , 


(3.16) 


where  U  =  I-2n  —  \Q  and  V  =  Using  the  inversion  formula  provided  in  [32] 
and  the  expression  of  A~l  given  by  (3.16),  it  follows  that  the  inverse  of  (3.13)  is 
computed  as 


I-W)  =  v2 


A-1  +  jA-VA-1 

1-0 

7 

1 

-\bTA~l 

1 

c 

\ 


=  a 


P(i,(x  Pfat  P^o 


Pt,ii  Pt,t  Pt,6 


\ 


y  Pe,ii  Pe,t  Pe,e  j 


(3.17) 


where  Q  =  c  —  bT A  1b  =  \nTQn  and  the  covariances  Pt  t  and  Pg $  are  given 
by  (3.11),  (3.12)  and  (3.10),  respectively.  □ 

The  performance  of  the  closed-form  ML  estimators  given  in  Theorem  1  relative 
to  the  Cramer- Rao  bounds  of  Theorem  3  is  shown  in  Fig.  3.1. 
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Figure  3.1:  Cramer- Rao  lower  bounds  and  performance  of  closed-form  MLE.  (top) 
A  plot  of  the  trace  of  the  CRLB  covariance  matrices  P^,  Pt,t  and  Po:e  derived 
from  the  parameters  //  (common  landmarks),  t  (translation)  and  6  (rotation), 
respectively,  is  shown  above  in  the  figure.  The  plots  are  shown  against  the  variance 
of  the  closed-form  ML  estimators  /i*,  t*  and  9*. 
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CHAPTER  4 

GENERALIZED  LIKELIHOOD  DATA  REGISTRATION 

In  a  general  mapping  scenario,  the  ground  truth  structure  observed  by  the 
agents  is  unknown.  In  particular,  if  the  Erst  two  entries  of  Xp  =  xp  correspond 
to  the  particular  landmark,  then  the  first  two  entries  of  Xq  =  xq  correspond  to  a 
different  landmark  in  general  (and  likewise  with  the  remaining  entries).  Common 
landmarks  in  this  case  are  identified  by  applying  a  matching  procedure  to  xp  and 
xq  with  consideration  that  the  stochastic  maps  are  obtained  in  separate  coordinate 
systems  related  by  6  and  t.  The  matching  procedure  proposed  in  this  section  is 
based  on  the  use  of  landmark  triplets  referred  to  as  triangles,  which  requires  that 
the  maps  of  each  agent  contain  at  least  three  landmarks.  An  overview  of  the 
registration  approach  is  shown  in  Fig.  4.1. 

4.1  Directed  hypergraph  model 

Triangles  are  constructed  from  the  maps  of  each  agent  by  following  a  direction 
convention  used  in  the  star-pattern  matching  approach  of  Groth  [24],  In  partic¬ 
ular,  given  three  landmark  locations  ya,yb,yc  £  M2,  the  Groth  representation  of 
a  directed  triangle  is  y  —  (y^,yJ,yl)T,  which  is  a  vector  in  M6  with  entries  that 
follow  the  inequality 

WVa-VbW  <  WVb-VcW  <  WVc-VaW  (4.1) 

under  the  assumption  that  no  two  triangle  edges  have  the  same  length.  This 
convention,  which  is  invariant  to  changes  in  rotation  and  translation  is  used  to 
construct  the  directed  hypergraphs  Gp  =  (VP,EP)  and  Gq  =  (Vq,  Eq)  from  the 
Delaunay  triangulations  of  maps  p  and  q,  respectively.  The  landmarks  that  form 
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(coordinate  system  p )  (coordinate  system  q ) 


_ t _ i _ 

Directed  hypergraph  model 


i _ t 


Likelihood  matrix  A 


_ I _ 

Matching  and  outlier  rejection 


Figure  4.1:  Overview  of  data  registration  approach.  Common  landmarks  and 
rigid-body  transform  parameters  are  determined  using  a  directed  hypergraph  rep¬ 
resentation  of  metric  maps.  Likelihood  statistics  of  common  directed  triangles  are 
stored  in  a  matrix  A,  which  is  used  to  determine  common  landmarks  by  solving  a 
linear  assignment  problem.  The  solution  of  the  assignment  problem  is  partitioned 
into  inkers  and  outliers  using  a  quaternion  outlier  rejection  approach. 
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the  vertices  of  each  graph  are  contained  by  Vp  (agent  p)  and  Vq  (agent  q).  The 
resulting  directed  triangles  constructed  from  the  maps  of  p  and  q  are  contained  by 
the  hyperedges  Ep  and  Eq,  respectively,  with  each  directed  triangle  being  indexed 
in  order  of  increasing  perimeter. 

4.2  Hypergraph  hypothesis  testing 

Determining  common  landmarks  from  the  directed  triangles  of  Gp  and  Gq  is  con¬ 
sidered  as  a  binary  hypothesis  testing  problem.  Under  hypothesis  H0 ,  the  agents 
observe  the  ground  truth  directed  triangles  up,  vq  G  M6,  which  contain  a  maximum 
of  two  landmarks  in  common.  Under  hypothesis  H i,  the  agents  observe  a  common 
directed  triangle  5  G  M6  within  their  respective  coordinate  systems.  In  this  way, 
Hq  is  the  hypothesis  of  uncommon  triangles  and  Hi  is  the  hypothesis  of  common 
triangles.  The  mathematical  models  of  H0  and  H i  are  given  by 


respectively.  The  appropriate  matching  hypothesis  (i.e.,  H0  or  Hi)  for  the  directed 
triangle  data  Yp  =  yp  and  Yq  =  yq  is  initially  unknown.  Given  the  realizations  yp 
and  yq  from  the  stochastic  maps  of  p  and  q,  respectively,  the  matching  hypothesis 
is  determined  using  a  generalized  likelihood  ratio  test  (GLRT)  of  the  form 


A  (yP,yq) 


maxLi(<5,  f,  6)  „ 

S,t,0  v  '  m 

-  \ 

max  L0(i/p,  vq)  Ho 

Hp,Hq 


r 


(4.4) 
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Figure  4.2:  Monte  Carlo  performance  of  hypergraph  hypothesis  testing.  Receiver 
operating  characteristic  (ROC)  curves,  illustrated  above,  show  the  performance  of 
the  detecting  triangle  matches  at  various  levels  of  SNR.  Each  of  the  curves  are  plots 
of  the  probability  of  detecting  a  match  (Pd)  versus  the  probability  of  a  false  alarm 
(Pfa)-  The  dashed  line  in  the  lower  region  of  the  figure  indicates  the  performance 
of  a  random  gness. 

where  Lk  are  likelihood  functions  under  Hk,  with  k  G  {0, 1},  and  the  threshold 
r  is  selected  to  control  the  level  of  false  alarm.  The  likelihood  statistic  A (yp,yq) 
is  easily  computed  by  applying  Theorem  1.  The  performance  of  the  approach  in 
the  presence  of  noise,  as  illustrated  by  the  receiver  operating  characteristic  (ROC) 
curves  of  Fig.  4.2,  is  of  interest  dne  to  the  uncertain  nature  of  stochastic  maps. 
As  illustrated  in  the  figure,  the  performance  of  the  approach  degrades  gracefully 
with  increasing  noise  (the  signal-to- noise  ratio,  or  SNR,  is  discussed  in  Section  5). 
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4.3  Data  registration  and  environment  structure 

4.3.1  Natural  vs.  semi-structured  environments 

A  desirable  trait  of  any  data  fusion  approach  is  the  ability  to  maintain  performance 
in  a  variety  environments  and  with  different  types  of  sensors.  Data  obtained  from 
two  types  of  environments  and  sensors  are  considered  in  this  section,  each  of  which 
are  illustrated  in  Fig.  4.3.  A  natural  environment  with  data  obtained  from  a 
laser  sensor  is  shown  in  Fig.  4.3a.  In  such  an  environment,  natural  landmarks 
such  as  trees  are  used  to  build  a  rigid-body  model  from  data.  In  contrast,  a  semi- 
structured  environment  with  data  obtained  from  a  camera  sensor  is  shown  in  Fig. 
4.3b.  Landmarks  such  as  edges  and  corner  points  usually  aid  in  the  construction 
of  metric  maps  in  environments  of  this  type.  While  the  data  fusion  approach 
of  this  paper  makes  no  assumption  on  the  type  of  sensor  or  environment,  the 
benchmark  datasets  are  used  to  show  the  potential  application  of  the  approach  to 
rigid-body  data  generated  from  a  wide  range  of  environments  and  sensors.  The 
simulated  examples  of  Fig.  4.4  illustrate  two  classes  of  landmark  arrangements 
related  to  the  experimental  examples  of  Fig.  4.3.  From  a  registration  perspective, 
the  arrangement  of  landmarks  in  the  Victoria  Park  example  (Fig.  4.3a)  resembles 
samples  drawn  from  a  uniform  distribution  (Fig.  4.4a)  and  landmarks  in  the  DLR 
example  (Fig.  4.3b)  resemble  landmarks  arranged  in  a  deterministic  grid  with 
additive  noise  (Fig.  4.4b). 
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(b) 

Figure  4.3:  Benchmark  rigid-body  models  of  natural  and  semi-structured  environ¬ 
ments.  (a)  Victoria  Park  provides  an  example  of  a  natural  outdoor  environment 
(dataset  courtesy  of  the  University  of  Sydney).  Landmark  locations  in  the  envi¬ 
ronment  are  determined  using  a  laser  range  finder,  (b)  An  example  of  a  semi- 
structured  indoor  environment  is  provided  by  the  DLR  Institute  of  Robotics  and 
Mechatronics  building  (dataset  courtesy  of  the  LIniversity  of  Bremen).  In  contrast 
to  the  Victoria  Park  dataset,  a  camera  sensor  is  used  to  determine  the  locations 
of  landmarks  within  a  research  facility. 
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Figure  4.4:  Synthetic  models  of  natural  and  semi-structured  environments,  (a) 
Samples  drawn  from  a  uniform  distribution  provide  a  model  of  landmarks  within 
a  natural  environment,  (b)  Landmarks  in  a  semi-structured  environment  are  mod¬ 
eled  in  this  example  by  points  in  a  deterministic  grid  with  additive  noise.  Each 
of  the  examples  provide  simple  models  of  the  spatial  arrangement  of  landmarks  in 
different  types  of  environments. 


4.3.2  Hypergraph  registration 


An  illustration  of  the  hypergraph  model  as  it  relates  to  data  registration  is  shown 
in  Fig.  4.5.  The  figure  illustrates  the  determination  of  common  landmarks  by 
identifying  common  directed  triangles  across  the  coordinate  systems  of  each  hy¬ 
pergraph.  Using  Theorem  1,  a  metric  for  determining  whether  a  triangle  yp  G  Ep 
in  hypergraph  Gp  corresponds  to  a  triangle  yq  G  Eq  in  hypergraph  Gq  is  given  by 
the  likelihood  statistic 


A  (yP,yq) 


max Li(6,  t,  6) 

5,t,e  v  ' 

ma xL0(up,uq) 

Vp,Vq 


(4.5) 


where  the  parameter  5  G  M6  is  the  location  of  the  common  triangle  and  the  param¬ 
eters  up,  uq  G  M6  are  the  locations  of  uncommon  triangles.  The  metric  is  computed 
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in  closed  form  from  yp  and  yq  as  specified  by  Theorem  1  since 


ma xLi(5,t,9)  —  Li(8*  ,t*  ,9*) 

S,t,9 


(4.6) 


under  the  hypothesis  that  yp  and  yq  correspond  to  a  common  directed  triangle 


Figure  4.5:  Application  of  directed  hypergraph  model  to  data  registration,  (left) 
Illustrated  in  the  figure  are  rigid-body  models  of  a  natural  environment  shown 
in  three  separate  coordinate  systems,  (right)  Common  landmarks  represented 
in  different  coordinate  systems  are  determined  by  matching  directed  triangles 
constructed  from  Delaunay  triangulations  (common  directed  hyperedges  are  illus¬ 
trated  by  the  shaded  triangles).  Once  common  triangles  are  determined  between 
each  hypergraph,  rotation  and  translation  parameters  are  estimated  in  closed  form 
using  the  vertices  of  the  matched  triangles. 
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parameter  6  and 

max  Lq(up,  uq)  =  L0(u*,  u*)  (4.7) 

Vp,Vq  F  H 

under  the  hypothesis  that  yp  and  yq  correspond  to  two  separate  directed  triangle 
parameters  up  and  uq,  respectively  (in  the  later  case,  yp  and  yq  contain  only  two 
common  landmarks  at  best  and  no  common  landmarks  in  general).  Likelihood 
statistics  computed  from  Gp  and  Gq,  as  specified  by  (4.5),  are  stored  in  matrix 
form  as  A  =  [./{,(?/*,  y3q)\  with  entries 

[  ,  i  e  V  and  j  e  Q 

LA  (4.8) 

I  0  ,  o.w. 

where  V  =  {1,  2, ... ,  \Ep\}  and  Q  =  {1,2,...,  \Eq\}.  Constructing  the  matrix  A 
from  the  directed  triangles  of  Gp  and  Gq  leads  to  a  linear  assignment  problem 
[33,  34]  of  the  form 


m  m 


maximize 

(4.9) 

i=  1  j= 1 

subject  to 

m 

^2zij  =  h  J  =  1 

i=  1 

(4.10) 

m 

Y,  Zjj  -  1,  i  =  1, . . .  ,  m 

4—1 

(4.11) 

and 

J 

Zij  G  {0, 1}, 

(4.12) 

where  m  =  max( \Ep\,  \Eq\),  which  is  readily  solved  using  optimized  routines  such 
as  the  Jonker-Volgenant  algorithm  [35]  by  relaxing  the  integer  program  to  a  linear 
program  with  decision  variables  %  G  [0, 1].  An  illustration  of  the  approach  using 
the  Victoria  Park  and  DLR  benchmarks  is  shown  in  Fig.  4.6. 
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(c)  (d) 

Figure  4.6:  Registration  of  landmarks  in  natural  and  semi-structured  environ¬ 
ments.  (a)  Rigid-body  models  of  the  Victoria  Park  are  shown  in  the  coordinate 
systems  of  a  fusion  agent  p  and  a  contributing  agent  q.  (b)  Common  landmarks 
are  determined  by  solving  a  linear  assignment  problem  involving  directed  triangles 
(the  solution  of  the  assignment  problem  is  illustrated  in  the  figure  by  shaded  tri¬ 
angles  with  corresponding  color  gradients).  The  approach  is  also  illustrated  using 
the  DLR  dataset  with  rigid-body  models  shown  in  (c)  and  the  registration  solution 
shown  in  (d). 
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Using  the  solution  of  the  assignment  problem,  a  collection  C  =  {c1,  c2, . . . ,  cm} 
of  correspondence  variables  is  constructed  to  indicate  m  common  landmarks  de¬ 
rived  from  the  vertices  of  directed  triangles.  Rigid-body  transform  parameters 
are  then  estimated  by  applying  Theorem  1  to  compute  t*  and  9*  using  the  com¬ 
mon  landmarks  indicated  by  C.  The  data  registration  solution  is  thus  of  the  form 
7 Z  =  {C,t*,6*},  which  is  used  to  compute  a  combined  global  model  using  the  data 
fusion  rules  proposed  in  Section  2.4. 

An  illustration  of  the  data  fusion  approach  and  structure  of  the  matrix  A 
is  shown  in  Fig.  4.7.  The  data  fusion  examples  in  the  figure,  which  illustrate 
the  approach  using  the  Victoria  Park  and  DLR  benchmarks,  show  the  combined 
global  models  within  the  coordinate  system  of  the  fusion  agent  p  in  addition  to  the 
likelihood  structure  of  the  explored  environment.  Using  the  directed  hypergraph 
model,  the  structure  of  the  environment  (and  subsequently,  the  complexity  of 
the  registration  problem)  is  indicated  in  part  by  the  bandedness  of  the  likelihood 
matrix  A. 

4.4  Outlier  rejection 

The  solution  of  the  assignment  problem  provides  a  list  of  candidate  common  tri¬ 
angles  between  the  hypergraphs  of  each  agent.  Associated  with  each  triangle  as¬ 
signment  is  a  maximum  likelihood  estimate  of  the  parameters  6  and  t  derived  from 
the  vertices  of  the  triangles  in  order  to  compute  the  likelihood  matching  statistic 
(4.5).  Under  rigid  body  assumptions,  inkers  in  the  candidate  triangle  assignments 
are  indicated  by  rigid-body  rotation  and  translation  parameters  that  form  a  clus¬ 
ter  around  the  true  values  of  6  (in  radians)  and  t  (in  meters).  A  simple  approach 
to  obtain  a  composite  representation  of  6  and  t  is  to  compute  a  unit  quaternion 
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[36,  37]  of  the  form 


Q  = 


cos  -9  |  +  u  ( sin  -6 


(4.13) 


where  9  is  a  rotation  angle  about  the  rotation  axis  n  6  l3  of  unit  magnitude.  By 


(b) 


Figure  4.7:  Data  fusion  and  likelihood  structure.  An  illustration  of  anisotropic 
data  fusion  in  the  coordinate  system  of  a  fusion  agent  p  is  shown  above  using  the 
(a)  Victoria  Park  and  (b)  DLR  examples.  The  likelihood  matrix  A  in  each  case 
provides  an  indication  of  the  underlying  structure  of  the  environment.  In  contrast 
to  the  likelihood  matrix  of  the  DLR  environment,  the  matrix  A  of  the  Victoria 
Park  example  naturally  exhibits  a  banded  structure. 
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defining  the  rotation  axis  u  relative  to  the  unit  z-axis,  denoted  by  ez  =  (0,  0,  l)r, 
a  combined  representation  of  9  and  t  is  obtained  using  Algorithm  2,  where  the 
matrix  R(q )  is  a  3  x  3  rotation  computed  from  the  entries  of  q  =  (g0,  qx ,  qyi  qz)T  as 


R(q) 


( <&  +  ql  -  <£,  -  (i 
2  (qyqx  +  qoQz) 
y  2  (qzqx  ~  qoqy ) 


2 (qxqy  -  qoqz ) 

ql  -  ql  +  qy  -  q, 

2  (qzqy  +  q0qx ) 


2(qxqz  +  q0qy)  ^ 
2 (qyqz  -  q0qx) 

ql-ql-q2y  +  q2zj 


(4.14) 


The  resulting  representation  of  the  rigid-body  transform  parameters  is  a  3D  coor¬ 
dinate  p  G  M3  on  the  unit  sphere.  Inliers  in  candidate  triangle  assignments  are  thus 
indicated  by  unit  sphere  coordinates  that  form  a  cluster  around  the  coordinate  p 
computed  from  the  true  values  of  9  and  t.  Using  the  spherical  representation  of 
the  rigid-body  transform  parameters  provided  by  Algorithm  2,  inliers  and  outliers 
are  detected  by  applying  standard  random  sample  consensus  (RANSAC)  [38]  rou¬ 
tines  to  the  resulting  3D  points.  An  illustration  of  the  outlier  rejection  approach 
is  shown  in  Fig.  4.8.  Eliminating  outliers  in  triangle  assignments  leads  to  a  regis¬ 
tration  solution  given  by  7 Z*  =  {C* ,t* ,  9*},  where  C*  C  C  is  the  collection  of  inlier 
correspondence  variables  used  to  compute  the  parameters  t*  and  9*. 


Algorithm  2  Quaternion  representation  of  9  and  t 
Input:  Rotation  parameter  9  E  [— n,  7r]  and  translation  t  E  M2 
Output:  Unit  sphere  coordinate  p  E  M3,  unit  quaternion  q 

Application:  Outlier  rejection 


9  v  9 

2:  q  =  cos  — b  tj— jt  sm  - 

2  |  |u||  2 

3:  p  =  R(q)ez 
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Figure  4.8:  Identifying  inliers  and  outliers  in  hypergraph  registration,  (a)  Com¬ 
mon  triangles  between  the  hypergraphs  Gp  and  Gq,  determined  by  solving  a  linear 
assignment  problem,  are  indicated  in  the  figure  by  black  markers  superimposed  on 
a  likelihood  matrix  A.  (b)  Each  entry  of  the  assignment  solution  is  used  to  com¬ 
pute  an  estimate  of  the  rigid  body  parameters  t  and  6,  illustrated  above  using  a 
quaternion  representation  on  the  unit  sphere.  Inliers  (red  dots)  and  outliers  (black 
crosses)  are  determined  by  applying  random  sample  consensus  to  the  spherical 
representation,  (c)  For  illustration  purposes,  the  corresponding  rigid-body  param¬ 
eters  (classified  as  inliers  and  outliers)  are  plotted  in  order  of  decreasing  likelihood 
statistics. 
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CHAPTER  5 

NUMERICAL  EXAMPLES  AND  SIMULATIONS 
5.1  Victoria  Park  example 

An  illustration  of  the  proposed  ML  fusion  approach  is  shown  in  Fig.  5.1.  The 
ground  truth  landmarks  shown  in  the  figure  are  obtained  by  applying  the  sparse 
local  submap  joining  filter  (SLSJF)  proposed  by  Huang  et  al.  [39]  to  the  Victoria 
Park  dataset.  The  ground  truth  is  partitioned  into  two  vectors  up  and  uq  as 
models  of  the  ground  truth  landmark  locations  observed  by  agent  p  and  agent  q, 
respectively  (see  Chapter  2).  The  stochastic  maps  of  each  agent  are  generated  using 
the  additive  noise  model  discussed  in  Section  2.4  at  an  SNR  of  30dB.  The  rotation 
(in  radians)  and  translation  (in  meters)  applied  to  the  stochastic  map  agent  q  are 
6  =  0.7854  and  t  =  (100, 5)T,  respectively.  As  illustrated  in  the  figure,  6  and  t 
parameterize  a  spatial  transform  of  the  stochastic  map  of  agent  q  in  reference  to 
the  ground  truth  coordinate  frame  (i.e.,  the  coordinate  system  of  agent  p). 

The  directed  hypergraph  representation  of  each  stochastic  map  is  used  by  the 
GLR  matching  to  determine  the  common  directed  triangles  across  the  coordinate 
systems  of  the  agents  (using  the  Groth  convention  enables  to  the  determination 
of  common  landmarks  from  common  directed  triangles).  The  number  of  directed 
triangles  in  hypergraphs  p  and  q  are  \Ep\  =  340  and  \Eq\  =  304,  respectively.  Due 
to  the  uncertainty  of  the  stochastic  maps,  outlier  rejection  is  required  to  determine 
an  inlier  set  of  matching  triangles  (as  illustrated  in  Fig.  5.2).  Theorem  1  is  then 
applied  to  the  inlier  landmarks  to  compute  the  closed  form  MLEs  9*  =  0.7878  and 
t*  =  (100.0860, 4. 9120)t.  Maximum  likelihood  estimation  of  the  combined  map 
u*  =  (p*T,  v*T,  v*T)T  immediately  follows  from  Lemma  1  and  Theorem  1. 
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Figure  5.1:  Hypergraph  registration  (Victoria  Park  example):  ground  truth  and 
registration  solution.  (Ground  truth)  The  true  landmark  locations  observed  by 
agent  p  and  agent  q  are  indicated  by  crosses  (+)  and  circles  (o),  respectively.  The 
agents  observe  50  landmarks  in  common  (contained  by  the  vector  p)  with  agent  p 
observing  179  landmarks  (contained  by  up)  and  agent  q  observing  160  landmarks 
(contained  by  uq).  (Data  models)  The  individual  stochastic  maps  of  the  agents 
are  generated  using  an  additive  Gaussian  noise  model,  with  the  map  of  agent  q 
being  transformed  into  a  separate  coordinate  system  by  6  and  t.  (Hypergraph 
registration)  Using  linear  programming  and  outlier  rejection,  an  inlier  set  of  16 
common  directed  triangles  (shaded  in  gray)  are  used  to  estimate  the  common 
landmark  parameters  of  the  data  models. 
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Figure  5.2:  Hypergraph  registration  (Victoria  Park  example):  likelihood  statistics 
and  outlier  rejection.  Markers  are  used  to  indicate  entries  of  the  likelihood  matrix 
A  corresponding  to  (a)  the  true  directed  triangle  matches,  (b)  the  triangle  matches 
specified  by  the  linear  program,  (c)  fnlicr  matches  (black  circles)  are  indicated  by 
the  rigid-body  transform  parameters  that  cluster  around  the  true  values  of  t  and 
6,  with  outliers  (gray  crosses)  indicated  by  the  entries  that  fall  outside  the  cluster, 
(d)  After  applying  outlier  rejection  to  the  output  of  the  linear  program,  16  inlier 
matches  are  used  to  construct  the  registration  solution  TV. 
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5.2  Monte  Carlo  experiments 


This  section  considers  the  performance  of  the  data  fusion  rules  proposed  in  Section 
2.4  and  the  hypergraph  registration  approach  described  in  Section  4.3.  Monte  Carlo 
results  are  provided  by  considering  the  performance  of  the  approach  when  applied 
to  large  scale  experimental  models  (Fig.  4.3)  and  small  scale  synthetic  models 
(Fig.  4.4)  of  natural  and  semi-structured  environments  over  a  wide  range  of  noise 
and  landmark  overlap,  as  illustrated  in  Fig.  5.3. 

The  Monte  Carlo  performance  of  the  proposed  data  registration  approach  (see 
Section  4.3)  is  evaluated  over  varying  levels  of  landmark  overlap  and  noise  using  the 
ground  truth  hypergraph  models  shown  in  Fig.  5.4.  The  performance  is  evaluated 
using  both  synthetic  and  experimental  ground  truth  models,  which  exhibit  a  similar 
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(a)  (b) 
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Figure  5.3:  Illustration  of  overlap  and  noise  models.  Hypergraph  models  are  shown 
above  for  a  fusion  agent  p  (left)  and  a  data  contributor  q  (right)  in  various  overlap 
and  noise  scenarios.  The  top  row  illustrates  common  triangles  (red)  and  uncommon 
triangles  (white)  in  hypergraphs  of  (a)  100%,  (b)  60%,  (c)  40%  and  (d)  0%  overlap. 
The  bottom  row  (e-h)  illustrates  the  impact  of  noise  at  a  fixed  overlap  of  100%. 
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structure  of  the  likelihood  matrix  A  (bottom  row  of  Fig.  5.4).  The  edge  lengths 
{£;}  of  each  graph  are  used  to  compute  the  signal  variance  given  by  a2  =  —  Y^i=\  ^  ■ 
A  noise  variance  is  computed  as  a2  =  2 a2  since  zero-mean  Gaussian  noise  is  added 
to  the  individual  vertices  of  each  edge  with  a  variance  of  a2.  Using  the  signal 
variance  cr2  and  noise  variance  cr2,  the  signal-to-noise  ratio  (SNR)  in  decibels  is 
then  SNRdB  =  10  log(cr^/cr^).  The  noise  level  of  each  Monte  Carlo  experiment 
is  thus  specified  by  solving  for  the  noise  variance  cr2  as  a  function  of  the  signal 
variance  and  SNR. 


(a)  (b)  (c)  (d) 

Figure  5.4:  Ground  truth  models  and  likelihood  structure.  The  top  row  shows 
Delaunay  triangulations  of  ground  truth  points  computed  from  (a)  100  points 
(189  triangles)  drawn  from  a  uniform  distribution,  (b)  100  points  (185  triangles) 
arranged  in  a  deterministic  grid  with  additive  noise,  (b)  the  Victoria  Park  example 
with  299  points  (576  triangles)  and  (d)  the  DLR  example  with  549  points  (1073 
triangles).  The  bottom  row  shows  the  likelihood  matrix  A  associated  with  each 
model.  The  figure  illustrates  similarities  in  A  between  the  natural  environment 
models  of  (a)  and  (c)  and  the  semi-structured  models  of  (b)  and  (d). 
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Four  metrics  of  performance  are  illustrated  in  Fig.  5.5: 


1.  the  true  percentage  of  common  triangles  relative  to  the  ground  truth  model 
of  the  environment, 

2.  the  percentage  of  true  positive  triangle  assignments  determined  by  the  solu¬ 
tion  of  the  linear  assignment  problem, 

3.  the  absolute  difference  (normalized  as  a  percentage)  indicating  the  difference 
between  the  true  percentage  of  common  triangles  and  the  percentage  true 
positives  and 

4.  the  percentage  of  matched  (inker)  triangle  assignments  (relative  to  the  true 
number  of  common  triangles)  after  applying  outlier  rejection. 

The  results  indicate  a  graceful  degradation  in  performance  with  decreasing 
SNR.  However,  the  approach  begins  to  break  down  in  the  noise  range  of  SNRjb  < 
20  dB  due  to  either  a  lack  of  (or  inability  to  detect)  common  triangles.  The 
resulting  data  fusion  mean  square  error  (MSE)  relative  to  the  coordinate  system 
of  fusion  agent  is  shown  in  Fig.  5.6. 
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Figure  5.5:  Monte  Carlo  performance  of  hypergraph  matching.  Varying  the  levels 
of  noise  and  overlap  results  in  a  change  in  (a)  the  percentage  actual  common 
triangles  relative  to  the  ground  truth  of  Gp  and  Gq  and  (b)  the  number  of  true 
positive  common  triangles  determined  by  the  linear  assignment  problem  (LAP). 
The  absolute  difference  between  (a)  and  (b),  normalized  as  a  percentage,  is  shown 
in  (c),  which  shows  that  the  difference  between  the  LAP  solution  and  ground 
truth  is  small  for  a  reasonably  wide  range  of  overlap  and  noise,  resulting  in  the 
percentages  of  matched  triangles  after  outlier  rejection  shown  in  (d). 
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SNR  (dB) 

Figure  5.6:  Monte  Carlo  MSE  (in  dB)  performance.  The  plot  shows  that  in  the 
range  of  15%-100%  overlap  in  landmarks,  the  data  fusion  MSE  exhibits  a  linear 
degradation  in  performance  with  decreasing  SNR.  However,  the  data  fusion  MSE 
is  relatively  high  at  all  noise  levels  below  a  15%  overlap  in  landmarks  due  to  a 
general  lack  of  common  triangles. 
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CHAPTER  6 

CONCLUSION 

This  thesis  considered  the  problem  of  constructing  a  global  map  of  landmarks 
from  the  data  models  contributing  agents.  The  problem  can  be  formulated  as  a 
mixed  integer-parameter  estimation  problem  from  which  landmarks  common  to  a 
fusion  agent  and  data  contributor  are  registered  under  a  global  coordinate  system. 
Under  this  framework,  the  optimal  fusion  of  stochastic  maps  can  be  accomplished 
using  the  maximum  likelihood  principle.  Unfortunately,  however,  the  complexity 
of  the  true  ML  solution  is  prohibitive,  which  leads  to  a  partitioning  of  data  fusion 
into  two  main  parts:  (i)  data  registration  using  a  bipartite  directed  hypergraph 
matching  using  generalized  likelihood  ratio  statistics  and  (ii)  parameter  estimation 
to  obtain  the  estimated  of  the  combined  map  under  a  common  coordinate  frame. 

The  main  advantage  of  the  proposed  approach  is  the  comprehensive  nature 
of  the  procedure:  a  global  map  is  found  in  spite  of  the  individual  data  models 
being  obtained  in  separate  coordinate  systems  without  prior  knowledge  of  common 
landmarks.  The  performance  of  the  proposed  approach  is  found  to  be  reasonable 
at  high  SNR  but  deteriorates  gracefully  with  increasing  noise,  which  is  largely  due 
to  a  decrease  in  common  directed  triangles.  One  way  to  improve  the  performance 
would  be  to  impose  neighborhood  and  adjacency  constraints  on  the  hypergraph 
model.  Further  computation  gains  may  also  be  found  by  exploiting  the  banded 
structure  of  the  likelihood  matrix  A. 

With  continual  advances  in  sensors  and  computational  ability,  the  future  of 
mobile  robotics  is  bright.  With  any  hope,  the  techniques  proposed  in  this  thesis  will 
help  pave  the  way  to  increased  autonomy  in  multi-agent  perception  and  navigation 
systems,  as  shown  in  Fig.  6.1. 
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Figure  6.1:  The  author  with  several  robotic  agents.  In  the  absence  of  a  global  frame 
of  reference,  a  global  metric  model  of  an  environment  is  constructed  from  the  data 
models  of  multiple  robotic  agents  (such  as  those  pictured  above)  by  applying  data 
fusion  techniques  of  the  nature  proposed  in  this  thesis. 


58 


73 


APPENDIX 

A.l  Review  of  sigma  point  filter 

This  section  reviews  the  sigma  point  filter  [40]  (illustrated  in  Fig.  A.l)  as  an 
example  algorithm  for  constructing  metric  data  models  of  an  environment.  Sigma 
points,  denoted  by  , ,  44  •  •  •  i  44  where  k  is  a  time  index,  are  variables 
drawn  from  the  multivariate  normal  distribution  using  a  deterministic  sigma  point 
sampler  given  by 


Xk~  i  for  i  =  0 

xk-i  +i/(L  + A)Ejjfc_i  for  i  =  l,...,L 
{  xk~  i  -i/  (L  +  A)  T,i-L,k- 1  for  *  =  L  +  1,...,2L 


where  L  is  the  dimension  of  the  state  estimate  (i.e.,  G  ML)  and  the  parameter 
A  =  a2(L  +  k)  —  L  determines  the  spread  of  the  sigma  points.  The  notation 
denotes  column  i  of  the  covariance  matrix  £&_!•  Sigma  points  are  also 
characterized  by  the  weights 

42  =  A  (L  +  A)"1 
4°'  =  A (L  +  A)-1  +  (1  -  a2  +  (3) 

Wm  =  4^  =  [2(T  +  A)]-1  for  i  =  l,...,2L 

where  w\\  and  4'^  are  weights  associated  with  mean  and  covariance,  respectively 
(choose  /3  =  2  for  Gaussian  distributions  [40]).  Using  the  notation  7  =  \J [L  +  A) 
for  brevity,  the  sigma  point  filter  (also  known  as  the  unscented  Kalman  filter ,  or 
UKF),  is  specified  given  a  nonlinear  process  model  /(•),  with  control  input  Uk  at 
time-step  k,  and  nonlinear  measurement  model  h(-)  as  follows. 
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%k—l  S k—1  Wfc 


Figure  A.l:  Block  diagram  of  sigma  point  filter. 

A.  1.1  Sample 

Xk-i  =  ^fc-i  f°r  i  =  0 

=  ®fc-i  +  7\/Vu  for  i  =  l,...,L 

A-i  =  ®fc-i  -  7VS*-L,fc-1  f°r  ?'  =  F  +  1,  •  •  • ,  2L 

A. 1.2  Predict 

•rI;Li  =  /(4'ii.«*)  for  i  =  0,...,2I 

2  L 

** = e™m;l 

i=0 

2L  rp 

Sfc  =  (^7c|fc-l  —  — 
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A. 1.3  Correct 

Sigma-point  update 

X§h  =  xk  for  i  =  0 

X k\k  =  +  l\f%M  for  i  =  1,  •  •  • ,  L 

=  for  i  =  L  +  1,. . .  ,2L 

Measurement  prediction 


ylF  =  hlXjgk)  for  i  —  0, . . .  ,2L 


2  L 


yk  = 


i= 0 


Kalman  update  (with  measurement  noise  covariance  Rk) 


2  L 


i= 0 
2  L 


St  =  Yl (Jf  -  yA  (5f  -y A'  +R. 


wt  =  (  X  wf  (4;1  -  **)  (jf  -  ykY  ) 


,i= 0 


Xk  =  Xk  +  Vhfe  (j/fc  -  yk) 


sfc  =  sfc  -  wkskwj:. 
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